#include <QLCDNumber>

#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "types.h"
#include "mygraphicsscene.h"


#include "Aria.h"
#include "ArNetworking.h"

#include <QGraphicsItemGroup>
#include <QGraphicsEllipseItem>
#include <QListWidgetItem>
#include <QString>
#include <vector>
#include <QMutex>


#include <iostream>



MainWindow::MainWindow(QWidget *parent,ArClientBase *cliente) :
    QMainWindow(parent),
    ui(new Ui::MainWindow),
    getSafeDriveCB(this,&MainWindow::getSafeDrive),
    getAutoDockCB(this,&MainWindow::getAutoDock)
{

    ui->setupUi(this);

    client = cliente;

    grascene = new myGraphicsScene(this,client);




    ui->graphicsView->setScene(grascene);

    locWindow = new LocalizationWindow(this,client);
    connect(this,SIGNAL(signal_changed_localization_button(bool)),locWindow,SLOT(slot_changed_localization_button(bool)));
    connect(locWindow,SIGNAL(signal_changed_localization_button(bool)),this,SLOT(slot_changed_localization_button(bool)));




    LCD_voltage_actual.setAutoFillBackground(true);// see the different if you comment that line out.
    LCD_voltage_actual.setSegmentStyle(QLCDNumber::Flat);

    QPalette Palon = LCD_voltage_actual.palette();
    Palon.setColor(QPalette::Normal, QPalette::WindowText, Qt::green);
    Palon.setColor(QPalette::Normal, QPalette::Window, Qt::black);
    LCD_voltage_actual.setPalette(Palon);

    LCD_voltage_actual.setDecMode();
    LCD_voltage_actual.setSmallDecimalPoint(true);
    LCD_voltage_actual.setNumDigits(6);

    LCD_voltage_actual.display(12.001);

    ui->statusbar->addWidget(&LCD_voltage_actual);


    LCD_voltage_warning.setAutoFillBackground(true);// see the different if you comment that line out.
    LCD_voltage_warning.setSegmentStyle(QLCDNumber::Flat);

    QPalette Palwar = LCD_voltage_warning.palette();
    Palwar.setColor(QPalette::Normal, QPalette::WindowText, Qt::yellow);
    Palwar.setColor(QPalette::Normal, QPalette::Window, Qt::black);
    LCD_voltage_warning.setPalette(Palwar);


    LCD_voltage_warning.display(0.1);

    ui->statusbar->addWidget(&LCD_voltage_warning);

    LCD_voltage_off.setAutoFillBackground(true);// see the different if you comment that line out.
    LCD_voltage_off.setSegmentStyle(QLCDNumber::Flat);

    QPalette Paloff = LCD_voltage_off.palette();
    Paloff.setColor(QPalette::Normal, QPalette::WindowText, Qt::red);
    Paloff.setColor(QPalette::Normal, QPalette::Window, Qt::black);
    LCD_voltage_off.setPalette(Paloff);


    LCD_voltage_off.display(0.1);

    ui->statusbar->addWidget(&LCD_voltage_off);



    if(client->dataExists("getSafeDrive")){
        client->addHandler("getSafeDrive",&getSafeDriveCB);
        client->requestOnce("getSafeDrive");
    }

    if(client->dataExists("getAutoDock")){
        client->addHandler("getAutoDock",&getAutoDockCB);
        client->requestOnce("getAutoDock");
    }



    if(!client->dataExists("physicalInfo")){
        hide_robot_controls();
        hide_battery_controls();
    }else{
        robot = new myRobot();
        grascene->addItem(robot);
        handleupdate = new handlerUpdate(this,client);
        connect(handleupdate,SIGNAL(signalUpdateNumbers(double,double,double,double,int,int)),this,SLOT(slotUpdateNumbers(double,double,double,double,int,int)));
        connect(handleupdate,SIGNAL(signalUpdateStrings(char*,char*)),this,SLOT(slotUpdateStrings(char*,char*)));
        connect(handleupdate,SIGNAL(signalUpdateBatteryInfo(double,double)),this,SLOT(slotUpdatebatteryInfo(double,double)));
    }

    if(!client->dataExists("getGoals")) hide_goals();
    else{

        handlergoals = new handlerGoals(this,client);
        connect(handlergoals,SIGNAL(signal_add_goal(QString)),this,SLOT(slot_add_goal(QString)));
        connect(handlergoals,SIGNAL(signal_remove_goals()),this,SLOT(slot_remove_goals()));
    }

    if(!client->dataExists("getMap"));
    else{
        handlemap = new handlerMap(this,client);
        connect(handlemap,SIGNAL(signal_paint_ellipse(QGraphicsEllipseItem*)),this,SLOT(slot_draw_Ellipse(QGraphicsEllipseItem*)));
        connect(handlemap,SIGNAL(signal_paint_line(QGraphicsLineItem*)),this,SLOT(slot_draw_Line(QGraphicsLineItem*)));
        connect(handlemap,SIGNAL(signal_paint_goal(myGoalItem*)),this,SLOT(slot_paint_goal(myGoalItem*)));
        connect(handlemap,SIGNAL(signal_remove_goal(myGoalItem*)),this,SLOT(slot_remove_goal(myGoalItem*)));
        connect(handlemap,SIGNAL(signal_paint_rect(QGraphicsRectItem*)),this,SLOT(slot_paint_rect(QGraphicsRectItem*)));
        connect(handlemap,SIGNAL(signal_remove_rect(QGraphicsRectItem*)),this,SLOT(slot_remove_rect(QGraphicsRectItem*)));
    }

    if(!client->dataExists("getSensorList"));
    else{
        handlesensor = new handlerSensor(this,client);
        connect(handlesensor,SIGNAL(signal_paint_ellipse(QGraphicsEllipseItem*)),this,SLOT(slot_draw_Ellipse(QGraphicsEllipseItem*)));
        connect(handlesensor,SIGNAL(signal_remove_ellipse(QGraphicsEllipseItem*)),this,SLOT(slot_remove_Ellipse(QGraphicsEllipseItem*)));
    }

    if(!client->dataExists("getPath"));
    else{
        handlepath = new handlerPath(this,client);
        connect(handlepath,SIGNAL(signal_paint_line(QGraphicsLineItem*)),this,SLOT(slot_draw_Line(QGraphicsLineItem*)));
        connect(handlepath,SIGNAL(signal_remove_line(QGraphicsLineItem*)),this,SLOT(slot_remove_Line(QGraphicsLineItem*)));
    }




    if(client->dataExists("getPictureCam1") || client->dataExists("sendVideo") || client->dataExists("getCameraInfo") || client->dataExists("getCameraList"))
      {
        grascene_video = new QGraphicsScene();
        ui->graphicsView_3->setScene(grascene_video);


        if(!client->dataExists("getPictureCam1") && !client->dataExists("sendVideo")){
            grascene_video->addSimpleText("No Video Data");
        }




        handlecamera = new handlerCamera(this,client);

        connect(handlecamera,SIGNAL(signal_no_camera()),this,SLOT(slot_no_camera_exits()));
        connect(handlecamera,SIGNAL(signal_paint_image(uchar *,int)),this,SLOT(slot_paint_image(uchar *,int)));
        connect(handlecamera,SIGNAL(signal_get_camera_values(int,int,int,int,int,int,int)),this,SLOT(slot_get_camera_values(int,int,int,int,int,int,int)));
        connect(this,SIGNAL(signal_update_camera(int,int,int)),handlecamera,SLOT(slot_updates_camera(int,int,int)));





    }else hide_camera();



    connect(grascene,SIGNAL(signal_paint_goal(myGoalItem*)),this,SLOT(slot_paint_goal(myGoalItem*)));
    connect(grascene,SIGNAL(signal_remove_goal(myGoalItem*)),this,SLOT(slot_remove_goal(myGoalItem*)));




    client->runAsync();
    ui->graphicsView->scale(0.4,0.4);

}

MainWindow::~MainWindow()
{
    //client.disconnect();
    Aria::shutdown();

    delete ui;
}

void MainWindow::on_actionQuit_triggered()
{
    exit(EXIT_SUCCESS);
}

void MainWindow::on_checkBox_Hide_Controls_stateChanged(int arg1)
{

        ui->groupBox_Camera_Controls->setVisible(!ui->checkBox_Hide_Controls->isChecked());

}

void MainWindow::on_actionVideo_changed()
{
    ui->widgetCamera->setVisible(ui->actionVideo->isChecked());
}

void MainWindow::on_actionGoals_changed()
{
    ui->widgetGoals->setVisible(ui->actionGoals->isChecked());
}

void MainWindow::on_actionRobot_Tools_changed()
{
    ui->toolBarRobot->setVisible(ui->actionRobot_Tools->isChecked());
}

void MainWindow::on_actionMap_Tools_changed()
{
    ui->toolBarMap->setVisible(ui->actionMap_Tools->isChecked());
}

void MainWindow::on_actionAdvanced_Tools_changed()
{
    ui->toolBarAdvance->setVisible(ui->actionAdvanced_Tools->isChecked());
}

void MainWindow::on_actionMap_changed()
{
    ui->groupBox_Map->setVisible(ui->actionMap->isChecked());
}


void MainWindow::slot_draw_Ellipse(QGraphicsEllipseItem *item){

    grascene->addItem(item);

}




void MainWindow::slot_draw_Line(QGraphicsLineItem *item){

    grascene->addItem(item);

}


void MainWindow::slot_remove_Ellipse(QGraphicsEllipseItem *item){


    grascene->removeItem(item);

}




void MainWindow::slot_remove_Line(QGraphicsLineItem *item){

    grascene->removeItem(item);

}


void MainWindow::slot_scene_changed(QRectF rec){

    grascene->update(rec);

}


void MainWindow::slot_add_goal(QString goal){

    ui->listGoals->addItem(goal);
}



void MainWindow::slot_remove_goals(){

    ui->listGoals->clear();
}



void MainWindow::on_actionFit_in_Window_activated()
{
    ui->graphicsView->fitInView(grascene->sceneRect(),Qt::KeepAspectRatio);
}



void MainWindow::hide_robot_controls(){
    ui->toolBarRobot->setVisible(false);
    ui->actionStop->setVisible(false);
    ui->actionCome_Here->setVisible(false);
    ui->actionDock->setVisible(false);
    ui->actionUndock->setVisible(false);
    ui->actionHome->setVisible(false);
    ui->actionTour_Goals->setVisible(false);
    ui->actionWander->setVisible(false);
    ui->actionJoystick_Robot->setVisible(false);
    ui->actionDrive->setVisible(false);

}

void MainWindow::hide_battery_controls(){
    this->LCD_voltage_actual.setVisible(false);
    this->LCD_voltage_off.setVisible(false);
}

void MainWindow::hide_camera(){
    ui->widgetCamera->hide();
    ui->actionVideo->setVisible(false);
}

void MainWindow::hide_goals(){
    ui->widgetGoals->setVisible(false);
    ui->actionGoals->setVisible(false);
}

void MainWindow::on_button_goto_clicked()
{
    if(!ui->listGoals->selectedItems().count()) return;

    client->requestOnceWithString("gotoGoal", ui->listGoals->selectedItems().takeFirst()->text().toUtf8().constData());
}

void MainWindow::on_actionReduce_triggered()
{
    ui->graphicsView->scale(0.8,0.8);
}

void MainWindow::on_actionMagnify_triggered()
{
    ui->graphicsView->scale(1.2,1.2);
}

void MainWindow::on_actionDrive_triggered(bool checked)
{
    if(checked){

        wdrive = new Drive(this,client);
        connect(wdrive,SIGNAL(destroyed()),this,SLOT(slotHideDrive()));
        wdrive->show();
    }else{

    }
}

void MainWindow::slotUpdateNumbers(double battery,double X, double Y, double theta, int vel, int rot_vel){
   robot->setX(X);
   robot->setY(Y*(-1));


   ui->mostrar_posx->setText(QString::number(X));
   ui->mostrar_posy->setText(QString::number(Y));
   ui->mostrar_heading->setText(QString::number(theta));
   ui->mostrar_vel->setText(QString::number(vel));

   LCD_voltage_actual.setDecMode();
   LCD_voltage_actual.setSmallDecimalPoint(true);
   LCD_voltage_actual.setNumDigits(6);

   LCD_voltage_actual.display(battery);

   if(theta>180)theta = 640-theta;
   else theta = 180 - theta;

   theta = ((int)theta+270) % 360;

   robot->setRotation(theta);

   if(ui->actionCenter_on_Robot->isChecked()) ui->graphicsView->centerOn(robot);

}

void MainWindow::slotUpdateStrings(char *mode, char *status){

    ui->mostrar_mode->setText(QString(mode));
    ui->mostrar_status->setText(QString(status));
}

void MainWindow::slotHideDrive(){
    ui->actionDrive->setChecked(false);
}

void MainWindow::slotUpdatebatteryInfo(double warning,double shutdown){

    LCD_voltage_warning.display(warning);
    LCD_voltage_off.display(shutdown);
}

void MainWindow::on_actionStop_triggered()
{
    if(client->dataExists("stop"))
        client->requestOnceWithString("stop","");
}

void MainWindow::on_actionWander_triggered()
{
    if(client->dataExists("wander"))
        client->requestOnceWithString("wander","");
}

void MainWindow::on_actionTour_Goals_triggered()
{
    if(client->dataExists("tourGoals"))
        client->requestOnceWithString("tourGoals","");
}

void MainWindow::on_actionHome_triggered()
{
    if(client->dataExists("home"))
        client->requestOnceWithString("home","");
}

void MainWindow::on_actionDock_triggered()
{
    if(client->dataExists("dock"))
        client->requestOnceWithString("dock","");
}

void MainWindow::on_actionSafe_Drive_triggered(bool checked)
{
    if(client->dataExists("setSafeDrive")){
        ArNetPacket p;
        p.byteToBuf(checked);

        client->requestOnce("setSafeDrive",&p);

    }
}

void MainWindow::on_actionAuto_Dock_triggered(bool checked)
{
    if(client->dataExists("setAutoDock")){
        ArNetPacket p;
        p.byteToBuf(checked);

        client->requestOnce("setAutoDock",&p);

    }
}

void MainWindow::on_actionSend_Robot_triggered(bool checked)
{
    if(checked) ui->graphicsView->setCursor(Qt::CrossCursor);
    else ui->graphicsView->setCursor(Qt::ArrowCursor);
    if(checked && ui->actionLocalize_to_Point->isChecked()){
        ui->actionLocalize_to_Point->setChecked(false);
        emit signal_changed_localization_button(false);

    }

    grascene->setSendRobot(checked);
}

void MainWindow::slot_paint_image(uchar *buffer,int len){

    QPixmap imagen;
    imagen.loadFromData(buffer,len,0,Qt::AutoColor);
    grascene_video->clear();
    grascene_video->addPixmap(imagen);
    ui->graphicsView_3->fitInView(grascene_video->sceneRect(),Qt::KeepAspectRatio);



}


void MainWindow::slot_get_camera_values(int minPan,int maxPan,int minTilt,int maxTilt,int minZoom,int maxZoom,int haveZoom){

    ui->sliderpan->setMinimum(minPan);
    ui->sliderpan->setMaximum(maxPan);

    ui->slidertilt->setMinimum(minTilt);
    ui->slidertilt->setMaximum(maxTilt);

    ui->sliderzoom->setMinimum(minZoom);
    ui->sliderzoom->setMaximum(haveZoom);

    printf("Valores actualizados\n");
}

void MainWindow::on_sliderpan_valueChanged(int value)
{
    ui->edit_pan->setText(QString::number(((double)value)/100));
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}

void MainWindow::on_slidertilt_valueChanged(int value)
{
    ui->edit_tilt->setText(QString::number(((double)value)/100));
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}

void MainWindow::on_sliderzoom_valueChanged(int value)
{
    ui->edit_zoom->setText(QString::number(((double)value)/100));
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}


void  MainWindow::slot_paint_goal(myGoalItem *item){
    grascene->addItem(item);
}

void  MainWindow::slot_remove_goal(myGoalItem *item){
    grascene->removeItem(item);
}



void  MainWindow::slot_paint_rect(QGraphicsRectItem *item){
    grascene->addItem(item);
}

void  MainWindow::slot_remove_rect(QGraphicsRectItem *item){
    grascene->removeItem(item);
}



void MainWindow::on_actionLocalize_to_Point_triggered(bool checked)
{
    if(checked && ui->actionSend_Robot->isChecked()) ui->actionSend_Robot->setChecked(false);
    if((locWindow->isHidden()) && checked){

        locWindow->show();
        emit signal_changed_localization_button(true);

    }else{
        emit signal_changed_localization_button(checked);
    }

    grascene->setLocateRobot(checked);
    if(checked) ui->graphicsView->setCursor(Qt::CrossCursor);
    else ui->graphicsView->setCursor(Qt::ArrowCursor);
}

void MainWindow::slot_changed_localization_button(bool checked){
    if(checked && ui->actionSend_Robot->isChecked()) ui->actionSend_Robot->setChecked(false);
    ui->actionLocalize_to_Point->setChecked(checked);
    grascene->setLocateRobot(checked);
    if(checked) ui->graphicsView->setCursor(Qt::CrossCursor);
    else ui->graphicsView->setCursor(Qt::ArrowCursor);
}

void MainWindow::getAutoDock(ArNetPacket *packet){
    int present = packet->bufToUByte();

    ui->actionAuto_Dock->setChecked(present);
}

void MainWindow::getSafeDrive(ArNetPacket *packet){
    int present = packet->bufToUByte();

    ui->actionSafe_Drive->setChecked(present);
}

void MainWindow::slot_no_camera_exits(){
    hide_camera();
}

void MainWindow::on_button_Reset_Camera_clicked()
{
    ui->sliderpan->setValue(0);
    ui->slidertilt->setValue(0);
    ui->sliderzoom->setValue(0);
    emit signal_update_camera(0,0,0);
}



void MainWindow::on_pushButton_3_clicked()
{
    if(ui->sliderpan->value() > ui->sliderpan->minimum()) ui->sliderpan->setValue(ui->sliderpan->value() - ui->sliderpan->singleStep());
}

void MainWindow::on_pushButton_4_clicked()
{
    if(ui->sliderpan->value() < ui->sliderpan->maximum()) ui->sliderpan->setValue(ui->sliderpan->value() + ui->sliderpan->singleStep());
}

void MainWindow::on_pushButton_5_clicked()
{
    if(ui->slidertilt->value() > ui->slidertilt->minimum()) ui->slidertilt->setValue(ui->slidertilt->value() - ui->slidertilt->singleStep());
}

void MainWindow::on_pushButton_6_clicked()
{
    if(ui->slidertilt->value() < ui->slidertilt->maximum()) ui->slidertilt->setValue(ui->slidertilt->value() + ui->slidertilt->singleStep());
}

void MainWindow::on_pushButton_7_clicked()
{
    if(ui->sliderzoom->value() > ui->sliderzoom->minimum()) ui->sliderzoom->setValue(ui->sliderzoom->value() - ui->sliderzoom->singleStep());
}

void MainWindow::on_pushButton_8_clicked()
{
    if(ui->sliderzoom->value() < ui->sliderzoom->maximum()) ui->sliderzoom->setValue(ui->sliderzoom->value() + ui->sliderzoom->singleStep());
}

void MainWindow::on_sliderpan_rangeChanged(int min, int max)
{
    if(min<0 && max>0) ui->sliderpan->setValue(0);
    else
    ui->sliderpan->setValue(((max-min)/2)+min);
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}

void MainWindow::on_slidertilt_rangeChanged(int min, int max)
{
    if(min<0 && max>0) ui->slidertilt->setValue(0);
    else
    ui->slidertilt->setValue(((max-min)/2)+min);
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}

void MainWindow::on_sliderzoom_rangeChanged(int min, int max)
{
    if(min<0 && max>0) ui->sliderzoom->setValue(0);
    else
    ui->sliderzoom->setValue(((max-min)/2)+min);
    emit signal_update_camera(ui->sliderpan->value(),ui->slidertilt->value(),ui->sliderzoom->value());
}
